#include "buscan.hpp"
#include "devcan.hpp"

#define LOG_TAG "driver_bancan"
#define LOG_LVL LOG_LVL_DBG
#include <ulog.h>

robot_can_bus* robot_can_bus::p = RT_NULL;

const char* can_dev_name = "can1";

/* robot_can_bus functions define start */
/**
 * @brief  接口函数
 * @return robot_can_bus* 
 */
robot_can_bus* robot_can_bus::instance()
{
    if(p == RT_NULL)
    {
        p = new robot_can_bus(can_dev_name);
        p->open();
        return p;
    }
    else
    {
        return p;
    }
}

/**
 * @brief Construct a new robot can bus::robot can bus object
 * @param  name             My Param doc
 */
robot_can_bus::robot_can_bus(const char* name):robot_driver(name)
{
    rt_sem_init(&hw_rx_sem, "bscan", 0, RT_IPC_FLAG_FIFO);
    rt_sem_init(&s_sem, "bscan", 0, RT_IPC_FLAG_FIFO);
    head=RT_NULL;
}

/**
 * @brief Destroy the robot can bus::robot can bus object
 */
robot_can_bus::~robot_can_bus()
{
    rt_sem_detach(&hw_rx_sem);
    rt_sem_detach(&s_sem);
    rt_thread_delete(send_service_thread);
    rt_thread_delete(hw_rx_thread);
}

/**
 * @brief  friend function: thread for can sending message
 * @param  p                
 */
void service_thread_entry(void *p)
{
    robot_can_bus* thisp = static_cast<robot_can_bus*>(p);
    while(1)
    {
        rt_sem_take(&thisp->s_sem, RT_WAITING_FOREVER);
        can_msg temp = thisp->msg_f.pop();
        int wcount = thisp->robot_driver::write(0, temp.get_data_p(), sizeof(struct rt_can_msg));
		if(wcount == 0)
		{
			LOG_E("write can error");
		}
    }
}

/**
 * @brief friend function: thread for can receiveing message
 * @param  p                My Param doc
 */
void hw_rx_thread_entry(void *p)
{
    robot_can_bus* thisp = static_cast<robot_can_bus*>(p);
    struct rt_can_msg rxmsg = {0};
    while(1)
    {
        rt_sem_take(&thisp->hw_rx_sem, RT_WAITING_FOREVER);
        int rcount = thisp->read(0, &rxmsg, sizeof(rxmsg));
		if(rcount==0)
		{
			LOG_E("read from can error");
		}
        can_msg temp(&rxmsg);

        for(robot_list* ptr=thisp->head; ptr!=RT_NULL; ptr=ptr->next)
        {
            robot_can_dev* dev = static_cast<robot_can_dev*>(ptr->data);
            for(int i=0; i<dev->id_list_len; i++)
            {
                if(dev->id_list[i] == rxmsg.id)
                {
                    dev->msg.push(temp);
                    rt_sem_release(&dev->r_sem);
                }
            }
        }
    }
}

/**
 * @brief callback function for can receiveing
 * @param  dev              My Param doc
 * @param  size             My Param doc
 * @return rt_err_t 
 */
rt_err_t can_rx_call(rt_device_t dev, rt_size_t size)
{
    robot_can_bus* thisp = \
    static_cast<robot_can_bus*>(get_this_from_dev(dev));

    rt_sem_release(&thisp->hw_rx_sem);
    return RT_EOK;
}

/**
 * @brief 打开函数 在创建设备之后要打开
 */
void robot_can_bus::open()
{
    rt_device_open(hw_handle,RT_DEVICE_FLAG_INT_RX|RT_DEVICE_FLAG_INT_TX);
    rt_device_control(hw_handle, RT_CAN_CMD_SET_BAUD, (void*)CAN500kBaud);

    /* for debug */
    rt_device_control(hw_handle, RT_CAN_CMD_SET_MODE, (void*)RT_CAN_MODE_LOOPBACK);

    send_service_thread = rt_thread_create("scanbus",service_thread_entry,this,1024,10,10);
    if(send_service_thread != RT_NULL)
    {
        rt_thread_startup(send_service_thread);
    }

    hw_rx_thread = rt_thread_create("scanbus",hw_rx_thread_entry,this,1024,9,10);
    if(hw_rx_thread != RT_NULL)
    {
        rt_thread_startup(hw_rx_thread);
    }

    rt_device_set_rx_indicate(hw_handle, can_rx_call);
    add_this_to_dev(hw_handle, this);
}


/* robot_can_bus functions define end */




